// v1.0 07/07/13 created

#include "opencv2/objdetect/objdetect.hpp"
#include "opencv2/features2d/features2d.hpp"
#include "opencv2/nonfree/features2d.hpp"     // SURF & SIFT headers have been moved here!!!!
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/calib3d/calib3d.hpp"
#include "opencv2/imgproc/imgproc_c.h"

#include <iostream>
#include <vector>
#include <stdio.h>

#include "OpenCV/MatWork.hpp"
#include "FolderSparser.h"
#include "CoordTransf.hpp"

#include "PBVS.hpp"
#include "ZhangPoseEstimator.hpp"

using namespace std;
using namespace cv;

vector<Point2i> clk_pts;
int clk_cnt = 0;
Mat work_image;
int obj_flg, img_flg;

void RodriguesTest()
{
	Mat R = Mat::eye(3,3,CV_64F);
	Mat r = Mat::zeros(3,1,CV_64F);

	double ROL, PIT, YAW;
	ROL = 0;
	PIT = 0;
	YAW = 0;

	Mat tmp = roty(CV_PI/2.0)*rotx(-CV_PI)*rotz(CV_PI)*rotz(ROL)*roty(PIT)*rotx(-YAW);
	tmp(Range(0,3), Range(0,3)).copyTo(R);

	Rodrigues(R, r);

	printf("3x3 rotation matrix\n");
	MATmtxdisp(R);
	printf("3x1 rotation vector\n");
	MATmtxdisp(r);
	getchar();
}


void C170_projection_test()
{	
	PBVS pbvs; 

	Mat P = Mat::zeros(3,1,CV_64F);
	// 1st test
	P.at<double>(0) = 0.095;
	P.at<double>(1) = -0.065;
	P.at<double>(2) = 0.3;
	Mat p;
	pbvs.C170Project(P,p);
	MATmtxdisp(p);

	// 2nd test
	P.at<double>(0) = 0.053;
	P.at<double>(1) = 0.12;
	P.at<double>(2) = 0.453;
	pbvs.C170Project(P,p);
	MATmtxdisp(p);

	// 3rd test
	P.at<double>(0) = -0.053;
	P.at<double>(1) = 0.12;
	P.at<double>(2) = 0.453;
	pbvs.C170Project(P,p);
	MATmtxdisp(p);

	getchar();
}

// some frequent problem with mouse_clicking
void on_mouse( int event, int x, int y, int flags, void* param )  // this is same for both 2.3 & 2.1 versions
{
	//if(frame.empty()) exit;

	uchar ch1, ch2, ch3;	
	Point2i clk_pt;

	switch( event )
	{
		case CV_EVENT_LBUTTONDOWN:

			/*ch1 = ((uchar*)(frame->imageData + frame->widthStep*y))[x*3 + 0];
			ch2 = ((uchar*)(frame->imageData + frame->widthStep*y))[x*3 + 1];
			ch3 = ((uchar*)(frame->imageData + frame->widthStep*y))[x*3 + 2];*/

			/*ch1 = frameG.at<Vec3b>(y,x)[0];
			ch2 = frameG.at<Vec3b>(y,x)[1];
			ch3 = frameG.at<Vec3b>(y,x)[2];*/

			/*sampleG.x = ch1;
			sampleG.y = ch2;
			sampleG.z = ch3;*/

			// update the class
			//ClrDetG.SampleUpdate(sampleG);

			//printf("click point is (%d,%d) -> (R,G,B) = (%d,%d,%d)\n",y,x, sampleG.x, sampleG.y, sampleG.z);

			//writeTextFile();
			
			clk_cnt++;

			if (clk_cnt < 9)
			{
				clk_pt.x = x;
				clk_pt.y = y;

				clk_pts.push_back(clk_pt);

				// draw the click point
			}
			else
			{
				obj_flg = 1;
				img_flg = 1;
			}

			break;
		case CV_EVENT_LBUTTONUP:
			break;
	}
}

int main(int argc, char** argv)
{

	// ********************** START Zhang Pose Est
	ZhangPoseEstimator PoseEster;
	Mat rvec, tvec;
	Mat objPoints, imgPoints;

	// init obj points
	float objpts[12] = {0, 0, 0, 0, 0.075, 0, 0.075, 0.075, 0, 0.075, 0, 0};   // used for extrinsics est
	CvMat objptsmat = cvMat( 1, 4, CV_32FC3, objpts);
	objPoints = Mat(&objptsmat, false);

	// init img points
	imgPoints = Mat::zeros(1, 4, CV_32FC2);
	// ********************** END Zhang Pose Est

	// ********************** START mouse click
	char wndname[] = "Working Frame";
	namedWindow(wndname, 1);
	setMouseCallback( wndname, on_mouse, 0 );    // Mouse event init
	// ********************** END mouse click

	// ********************** START PBVS simulator
	obj_flg = 0, img_flg = 0;
	work_image = Mat::zeros(480,640,CV_8UC3);
	Mat cRo, cTo; 
	cRo = Mat::eye(4,4,CV_64F);
	cTo = Mat::eye(4,4,CV_64F);	
	// ********************** END PBVS simulator

	//C170_projection_test();


	char c;

	while (1)
	{
		// clear the image
		work_image = Mat::zeros(480,640,CV_8UC3);

		// Draw the points
		for (int i=0; i<clk_pts.size(); i++)
		{
			if (i<4)
			{
				// draw current points - draw green
				circle(work_image, clk_pts[i], 3, Scalar(0,255,0), 1);	
			}
			else 
			{
				// draw desire points - draw red
				circle(work_image, clk_pts[i], 3, Scalar(0,0,255), 1);
			}
		}
		
		imshow(wndname, work_image);
		
		// ***read key & response
		c = waitKey(1);

		// do 1 iteration of PBVS control
		if (c=='d')
		{

			// ********************* START Zhang Pose Estimation
			// Call Zhang module
			if (obj_flg && img_flg)
			{
				imgPoints.at<Vec2f>(0)[0] = clk_pts[0].x;
				imgPoints.at<Vec2f>(0)[1] = clk_pts[0].y;
				imgPoints.at<Vec2f>(1)[0] = clk_pts[1].x;
				imgPoints.at<Vec2f>(1)[1] = clk_pts[1].y;
				imgPoints.at<Vec2f>(2)[0] = clk_pts[2].x;
				imgPoints.at<Vec2f>(2)[1] = clk_pts[2].y;
				imgPoints.at<Vec2f>(3)[0] = clk_pts[3].x;
				imgPoints.at<Vec2f>(3)[1] = clk_pts[3].y;

				PoseEster.Estimate(objPoints, imgPoints, rvec, tvec);
				printf("rotation is:");
				MATmtxdisp(rvec);
				printf("translation is:");
				MATmtxdisp(tvec);

				rvec.copyTo(cRo(Range(0, 3), Range(0, 3)));   // RISK!!!
				tvec.copyTo(cTo(Range(0, 3), Range(3, 4)));
			}
			// *********************  End of Zhang Pose Est
		}


		// restart click points
		if (c=='r')
		{
			clk_cnt = 0;
			clk_pts.clear();

			obj_flg = 0;
			img_flg = 0;
		}		

		if (c==27) break;
		else
		{
		}
	}
	



    return 0;
}